#include <MsTimer2.h>

const byte pin1=2;
const byte pin2=3;
const byte pin3=4;
const byte pin4=5;
const byte pin5=7;
const byte pin6=6;
const byte pin7=8;
const byte pin8=9;

const byte ir_left = 13;
const byte ir_center = 11;
const byte ir_right = 12;

static volatile int stage1 = 0;
static volatile int stage2 = 0;
static volatile int angle = 0;

const int table[8][8] = {
  {0x1,0x0,0x0,0x0},
  {0x1,0x1,0x0,0x0},
  {0x0,0x1,0x0,0x0},
  {0x0,0x1,0x1,0x0},
  {0x0,0x0,0x1,0x0},
  {0x0,0x0,0x1,0x1},
  {0x0,0x0,0x0,0x1},
  {0x1,0x0,0x0,0x1}
};

//4拍
//const int table[8][8] = {
//  {0x1,0x0,0x0,0x0},
//  {0x1,0x0,0x0,0x0},
//  {0x0,0x1,0x0,0x0},
//  {0x0,0x1,0x0,0x0},
//  {0x0,0x0,0x1,0x0},
//  {0x0,0x0,0x1,0x0},
//  {0x0,0x0,0x0,0x1},
//  {0x0,0x0,0x0,0x1},
//};

void setup() {
  // put your setup code here, to run once:
  pinMode(pin1,OUTPUT);
  pinMode(pin2,OUTPUT);
  pinMode(pin3,OUTPUT);
  pinMode(pin4,OUTPUT);
  pinMode(pin5,OUTPUT);
  pinMode(pin6,OUTPUT);
  pinMode(pin7,OUTPUT);
  pinMode(pin8,OUTPUT);

  pinMode(11,INPUT);
  pinMode(12,INPUT);
  pinMode(13,INPUT);

  MsTimer2::set(2, go);
  MsTimer2::start();
}

void loop() {
    if(digitalRead(ir_center)==HIGH) {
      angle = 360;
    }
    else {
      angle = 0;
    }
}

void idelay(int t) {
  volatile int i,j = 0;
  for(i=0;i<t;i++)
    for(j=0;j<32767;j++);
}

int stage_inc(int s) {
  s++;
  if (s > 7)
    s = 0;

  return s;
}

int stage_dec(int s) {
  s--;
  if (s < 0 )
    s = 7;
  return s;
}

void step1(int s) {
    digitalWrite(pin1,table[s][0]);
    digitalWrite(pin2,table[s][1]);
    digitalWrite(pin3,table[s][2]);
    digitalWrite(pin4,table[s][3]);
}

void step2(int s) {
    digitalWrite(pin5,table[s][0]);
    digitalWrite(pin6,table[s][1]);
    digitalWrite(pin7,table[s][2]);
    digitalWrite(pin8,table[s][3]);
}

void go()
{
    step1(stage1);
    step2(stage2);
    
    if(angle < 10) {
      stage1 = stage_inc(stage1);
      stage2 = stage_inc(stage2);
    }
    else {
      stage1 = stage_dec(stage1);
      stage2 = stage_dec(stage2);
    }
}
